
#include	<math.h>

#include	"ImageType/ImageType.h"
#include	"Image2Tool.h"


#define	RC	32


void
image2_bilateral_step( image_type *sim, image_type *im, float beta, float sigma_x, float lamda )
{
image_type	*cim;
float	w[5];
int	wMat[50],	r,	*fp;
int	i,	j,	n,	m;
int	tmp;
short	*sp,	*sp0,	*tp,	*cp,	t0;

int	align,	align0;
int	k;

	guasian( 2, sigma_x, w );
	fp = &wMat[0];
	for( i = 0 ; i < 5 ; i++ )
		for( j = 0 ; j < 5 ; j++, fp += 2 ){

			if( i != 2 || j != 2 )
				*fp = lamda *w[i] * w[j] * 256;
			else	*fp = 0;
			*(fp+1) = - *fp;

		}


	cim = image_make_copy( sim, NULL );

	align = im->column - 5;
	align0 = 2*im->column + 2;

	for( i = 2; i < im->row-2 ; i++ ) {
		sp = (short*)IMAGE_PIXEL( im, i, 2 );
		cp = (short*)IMAGE_PIXEL( cim, i, 2 );
		tp = (short*)IMAGE_PIXEL( sim, i, 2 );
		for( j = 2; j < im->column-2 ; j++ ){


			fp = &wMat[0];
			sp0 = cp - align0;

			t0 = *cp;
			for( m = 0, r = 0 ; m < 5 ; m++, sp0 += align ){
				for( n = 0 ; n < 5 ; n++, sp0++, fp += 2 ){

					tmp = t0 - *(sp0);

//					if( tmp < 0 )	r -= *fp;
//					else	if( tmp > 0 )
//								r += *fp;


					k = ( tmp >> 31 ) &0x01;
					r += *(fp+k);

				}
			}
 
			r >>= 4;



			tmp = *sp++;
			tmp =  *tp -  ( tmp + r) *beta;
			*tp++ = tmp;

			cp++;
		}
	}


	image_destroy( cim, 1 );
}
